#include "high_includes.h"

PID_struct PID_power_st = PID_POWER_DEFAULTS;
PID_struct PID_temp_st = PID_TEMP_DEFAULTS;

/*
 * 函数介绍: PID初始化
 * 参数:
 * 返回值: {*}
 * 备注:
 */
void pid_init(PID_struct *v)
{
    v->Ui = 0;
    v->last_d = 0;
    v->SatErr = 0;
    v->OutPreSat = 0;
    v->Out = 0;
}

/*
 * 函数介绍: PID计算
 * 参数:
 * 返回值: {*}
 * 备注:
 */
void pid_func(PID_struct *v)
{
    // Compute the error
    v->Err = v->Ref - v->Fdb;

    // Compute the proportional output
    v->Up = v->Kp * v->Err;

    // Compute the integral output
    v->Ui = v->Ui + v->Ki * v->Up + v->Kc * v->SatErr;

    // Compute the derivative output
    v->Ud = v->Kd * (v->Up - v->Up1);

    // Compute the pre-saturated output
    v->OutPreSat = v->Up + v->Ui + v->Ud;

    // Saturate the output
    if (v->OutPreSat > v->OutMax)
    {
        v->Out = v->OutMax;
    }
    else if (v->OutPreSat < v->OutMin)
    {
        v->Out = v->OutMin;
    }
    else
    {
        v->Out = v->OutPreSat;
    }

    // Compute the saturate difference
    v->SatErr = v->Out - v->OutPreSat;

    // Update the previous proportional output
    v->Up1 = v->Up;
}

/*
 * 函数介绍: PID计算
 * 参数:
 * 返回值: {*}
 * 备注:
 */
void pid_func_new(PID_struct *v)
{
    // Compute the error
    v->Err = v->Ref - v->Fdb;

    // Compute the proportional output
    v->Up = v->Kp * v->Err;

    // Compute the derivative output
    /*v->Ud = v->Kd * (v->Up - v->Up1);*/
    /*  y = u * K where K = 1 / ( w * Ts ) */
    v->Ud = v->Kd * v->Err * (1.0 / v->Ts);

    // Compute the pre-saturated output
    v->OutPreSat = v->Up + v->Ui + (v->Ud - v->last_d);

    // Saturate the output
    if (v->OutPreSat > v->OutMax)
    {
        v->Out = v->OutMax;
    }
    else if (v->OutPreSat < v->OutMin)
    {
        v->Out = v->OutMin;
    }
    else
    {
        v->Out = v->OutPreSat;
    }

    // Compute the saturate difference
    v->SatErr = v->Out - v->OutPreSat;

    // Compute the integral output
    v->Ui += (v->Ki * v->Err + v->Kc * v->SatErr) * v->Ts;

    /* Update for Delay: '<S28>/UD' */
    v->last_d = v->Ud;
}

